
#include <string.h>


// Declare global variables
int MAX_COMMAND_SIZE = 10;
int MAX_NUM_PARAMS = 5;


void setup()
{
  // start serial port at 9600 bps:
  Serial.begin(9600);
  Serial1.begin(19200);
}



void loop()
{
  if (Serial.available()) {
    int positionCnt = 0;
    // Declare arrays to store parsed params
    char parm1[MAX_COMMAND_SIZE];
    char parm2[MAX_COMMAND_SIZE];
    char parm3[MAX_COMMAND_SIZE];
    char parm4[MAX_COMMAND_SIZE];
    char parm5[MAX_COMMAND_SIZE]; 
    
     // Declare buffer to receive serial data
    char buffer[MAX_COMMAND_SIZE * MAX_NUM_PARAMS + MAX_NUM_PARAMS]; 
    // Determines number of characters read into buffer
    int tempLen = Serial.readBytesUntil('\n',
      buffer, MAX_COMMAND_SIZE * MAX_NUM_PARAMS + MAX_NUM_PARAMS); 
    
    char command[tempLen + 1]; // Initialize array to store command
    
    // Transfer command from buffer into command array
    int i;
    for( i = 0; i < tempLen; i++) {  
      command[i] = buffer[i];
    }
    command[i + 1] = '\0';
    
    // Parse first word in command
    for(i = 0; i < tempLen && command[positionCnt] != ' ' && command[positionCnt] != '\0'; i++) {
      parm1[i] = command[positionCnt];
      positionCnt++;
    }
    parm1[i] = '\0';
    positionCnt++;

    if(strcmp(parm1,"p") == 0)
    {
      Serial.println("p");
    } else if(strcmp(parm1,"s") == 0) 
    {
      for(i = 0; i < (tempLen - positionCnt) && command[positionCnt] != ' '; i++) 
      {
        parm2[i] = command[positionCnt];
        positionCnt++;
      }
      parm2[i] = '\0';
      positionCnt++;

      for(i = 0; i < (tempLen - positionCnt) && command[positionCnt] != ' '; i++) 
      {
        parm3[i] = command[positionCnt];
        positionCnt++;
      }
      parm3[i] = '\0';
      positionCnt++;

      for(i = 0; i < (tempLen - positionCnt) && command[positionCnt] != ' '; i++) 
      {
        parm4[i] = command[positionCnt];
        positionCnt++;
      }
      parm4[i] = '\0';
      positionCnt++;

      for(i = 0; i <= (tempLen - positionCnt) && command[positionCnt] != ' '; i++) 
      {
        parm5[i] = command[positionCnt];
        positionCnt++;
      }
      parm5[i] = '\0';
      positionCnt++;

      Serial.println("s");
      
      //char buffer2[50];
      //sprintf(buffer2, "c %s %s %s %s", parm2, parm3, parm4, parm5);
      //Serial.println(buffer2);
      Serial.println("c 2 0 0");
      Serial.println("c 1 1.57 0");      
      Serial.println("c 1 1.57 1.57");      

      Serial.println("x");
    } else if(strcmp(parm1,"ready?") == 0){
      Serial.println("ready");
    }
    else if(strcmp(parm1,"r") == 0) 
    {
      char buffer_range[50];
      getRange(buffer_range);
      
      
      
      
      char buffer_send[50];
      sprintf(buffer2, "c %s",);
      Serial.println(buffer2);
    }
  }
  
}



void getRange()
{
  Serial1.print
}
